function [Tf,xf] = step(a)

clear tp tp0 yact ydes p data t
addpath('./build_torso')
addpath('./wrappers_torso')
addpath('./buildopt_torso')

%Simulation variables
g = 981/100;
ep = 50;


load('a_opt');
load('x_opt');
x0 = x_opt;
a = a_opt;



ic = resetFunc(x0);


%%%Variables used to store output data
global tp tp0 yact ydes p

tp0 = 0;
tp = [];
yact = [];
ydes = [];


%Initial position of the hip
% pos_ic = Pos(0,[0;0;ic]);
% p_hip_ic = pos_ic(1,3);
% p = [ p_hip_ic; a(1,1)];

p = phip_sca(ic);

sol = ode45(@(t,x) f1_vector(t,x,a,ep), [0 1], ic, ...
      odeset('Events', @(t,x) eventfcn(t,x,a), 'MaxStep', 1e-2));

% sol = ode45(@(t,x) f1_vector_temp(t,x,a,ep), [0 tau(q0,a)], ic, ...
%       odeset('MaxStep', 1e-2));



Tf = sol.x(end);
xf = sol.y(1:end,end);



% %Checking the output:
% R1 = [1 1 -1 -1;
%          0 0  0  1;
%          0 0 -1 0;
%          0 1  0 0]; 
%      
% qf = xf(1:4);
% qfplus = R1*qf;
% 
% %THESE SHOULD BE ZERO!!!!!!!
% y_minus(qf,a)
% y_plus(qfplus,a)

clear h 

yp = [];
ym = [];
for i = 1:length(sol.y)
     g(i) = h_sca(sol.y(1:end,i));
     yp = [yp, y_plus(sol.y(1:5,i),a)];
     ym = [ym, y_minus(sol.y(1:5,i),a)];
     h(i) = height(sol.x(i),a,p);
%      thetas(i) =  theta_sl(sol.x(i),a);
%      thetans(i) =  theta_nsl(sol.x(i),a);
%      ls(i)      =   l_sl(sol.x(i),a);
%      lns(i)     =   l_nsl(sol.x(i),a);
end


figure(1); clf;
plot(sol.x,h,'b+',sol.x,g,'g')
title('height of the guard (as a function of time) vs. the guard function')
legend({'height (closd form)','height (from integration)'});

% figure(2); clf;
% plot(sol.x,thetas,sol.x,thetans,sol.x,ls,sol.x,lns)
% legend({'\theta_{sl}','\theta_{nsl}','l_{sl}','l_{nsl}'});

figure(4); clf;
plot(tp,yact,'k',tp,ydes,'r',sol.x,g,'g')
title('ya vs yd')
xlabel('time (s)');
ylabel('desired vs. actual')
legend({'desired','actual'});

y = yact-ydes;
figure(5); clf;
plot(tp,y,'k',sol.x,g,'g')
title('y as computed through simulation')
xlabel('time (s)');
ylabel('desired vs. actual')
legend({'desired','actual'});

% figure(4); clf;
% plot(tp,yact(2,:),'k',tp,ydes(2,:),'r',sol.x,g,'g')
% title('ya vs yd')
% xlabel('time (s)');
% ylabel('desired vs. actual')
% legend({'desired','actual'});

end
% 
% 
% %%%%%%%%%%%%%% Determining the height of the foot from the canonical
% %%%%%%%%%%%%%% functions:
% 
% 
% function ret = height(t,a)
% 
% ret = l_sl(t,a)*cos(theta_sl(t,a)) - l_nsl(t,a)*cos(theta_nsl(t,a));
% 
% end
% 
% 
% 
% function ret = theta_nsl(t,a)
% 
% ret = atan(yd_ns_slope(t,a));
% 
% end
% 
% 
% function ret = theta_sl(t,a)
% 
% q0 = theta_a(a);
% qdot0 = H_minus(q0,a)\[a(1,1); 0; 0; 0];
% x0 = [q0; qdot0];
% 
% ic = resetFunc(0,x0);
% 
% pos_ic = Pos(0,[0;0;ic]);
% p_hip_ic = pos_ic(1,3);
% 
% p_hip = yd_hip_pos(t,a)+ p_hip_ic;
% 
% ret = asin(p_hip/l_sl(t,a));
% 
% end
% 
% 
% 
% function ret = l_nsl(t,a)
% 
% Lc = 43561/100000;
% Lt = 40134/100000;
% 
% ret = sqrt(Lt^2+Lc^2 - 2*Lt*Lc*cos(pi-yd_ns_knee(t,a)));
% 
% 
% end
% 
% 
% 
% 
% function ret = l_sl(t,a)
% 
% Lc = 43561/100000;
% Lt = 40134/100000;
% 
% ret = sqrt(Lt^2+Lc^2 - 2*Lt*Lc*cos(pi-yd_s_knee(t,a)));
% 
% end
% 
% 
% 
% %%%%%%%%%%%%%% Canonical human functions
% 
% function ret = yd_ns_slope(t,a)
% 
% ret =exp(-a(2,4)*t).*(a(2,1)*cos(a(2,2)*t)+a(2,3)*sin(a(2,2)*t))+a(2,5);
% 
% end
% 
% function ret = yd_hip_pos(t,a)
% 
% ret = a(1,1)*t;
% 
% end
% 
% function ret = yd_s_knee(t,a)
% 
% ret = exp(-a(3,4)*t).*(a(3,1)*cos(a(3,2)*t)+a(3,3)*sin(a(3,2)*t))+a(3,5);
% 
% end
% 
% function ret = yd_ns_knee(t,a)
% 
% ret = exp(-a(4,4)*t).*(a(4,1)*cos(a(4,2)*t)+a(4,3)*sin(a(4,2)*t))+a(4,5);
% 
% end



% 
% 
% 
% function ret = f1_vector_temp(t, q, a, ep)
% global tp tp0 yact ydes p
%   
% ya = [1:1:4];
% yd = [1:1:4];
%   
% dx = q(5:8);
% M    = Dmat(t, q); 
% C    = Cmat(t, q);
% G    = Gvect(t, q);
% F = 0; 
% 
% y_a     = ya_vec(t, q, p, a);
% y_d     = yd_vec(t, q, p,  a);
% Dy_a    = Dya_mat(t, q, p, a);
% Lfy_d   = Lfyd_vec(t, q, p,  a);
% DLfy_a  = DLfya_mat(t, q, p, a);
% DLfy_d  = DLfyd_mat(t, q, p, a);
% LfLfy_d = LfLfyd_vec(t, q, p, a);
% 
% 
% tp = [tp, t+tp0];
% yact = [yact, y_a];
% ydes = [ydes, y_d];
% 
% y = y_a(ya) - y_d(yd); 
% Dya = Dy_a(ya,:);
% DLfya = DLfy_a(ya,:);
% DLfyd = DLfy_d(yd,:);
% 
% % Vector field
% vf = [dx; M \ (F - C*dx - G)];
% vhf = [M \ (F - C*dx - G)];
% 
% 
% B_IO = eye(4);
% gf    = [zeros(size(B_IO)); M \ B_IO];
% Lfy = [y(1); Dya(2:end,:) * vf] - [0; Lfy_d(yd(2:end))];
% LfLfy = [Dya(1, :); DLfya(2:end, :)- DLfyd(2:end, :)] * vf - [Lfy_d(yd(1));0;0;0];
% A = [Dya(1,:); DLfya(2:4,:)- DLfyd(2:end, :)]*gf ;
% y = [0; y(2); y(3); y(4);];
% 
% u = -A \ (LfLfy + ep * Lfy + ep^2 * y); 
% 
% ret = vf + gf * u;
% end